<?xml version="1.0" encoding="utf-8"?>
<sdf version='1.6'>

{%- import 'mrs_robots_description/sdf/generic_components.sdf.jinja' as mrs_generic -%}

{# visual_cylinder_macro {--> #}
{%- macro visual_cylinder_macro(name, height, radius, color, x, y, z, roll, pitch, yaw) -%}
  {#- a generic macro to add a colored cylinder visual block. Must be placed inside a link block -#}
  <visual name="{{ name }}_visual">
    <pose>{{ x }} {{ y }} {{ z }} {{ roll }} {{ pitch }} {{ yaw }}</pose>
    <geometry>
      <cylinder>
        <length>{{ height }}</length>
        <radius>{{ radius }}</radius>
      </cylinder>
    </geometry>
    <material>
      <script>
        <name>Gazebo/{{ color }}</name>
        <uri>file://media/materials/scripts/gazebo.material</uri>
      </script>
    </material>
  </visual>
{%- endmacro -%}
{# <!--}--> #}

{# visual_cylinder_with_collision_macro {--> #}
{%- macro visual_cylinder_with_collision_macro(name, height, radius, color, x, y, z, roll, pitch, yaw) -%}
  {# a generic macro to add a colored cylinder with a collision hitbox. Must be placed inside a link block #}
  {{ visual_cylinder_macro(name, height, radius, color, x, y, z, roll, pitch, yaw) }}
  {{ mrs_generic.collision_cylinder_macro(name, height, radius, x, y, z, roll, pitch, yaw) }}
{%- endmacro -%}
{# <!--}--> #}

{# custom_monochrome_camera_macro {--> #}
{%- macro custom_monochrome_camera_macro(camera_name, parent_link, x, y, z, roll, pitch, yaw, spawner_args) -%}
  {#- an optional sensor that can be enabled and reconfigured by the MRS drone spawner -#}

  {#- spawner API hook {--> -#}
  {#- this part allows dynamic configuration of this component by the mrs_drone_spawner -#}
  {%- set spawner_keyword = 'enable-custom-monochrome-camera' -%} {# access this macro from the drone spawner #}
  {%- set spawner_description = 'Add a custom monochrome camera to the drone' -%} {# displayed when --help is called in the drone spawner #}
  {%- set spawner_default_args = {'update_rate': 45, 'noise': 0.004} -%} {# internal params that can be overriden in spawn-time #}
  {#- <!--} spawner API hook -#}

  {#- only include if the keyword was added to the spawn command -#}
  {%- if spawner_keyword in spawner_args.keys() -%}

    {#- setup default args, override values with user input if possible -#}
    {{ mrs_generic.handle_spawner_args(spawner_keyword, spawner_default_args, spawner_args) }}
    {#- we now have data in spawner_args[spawner_keyword]['update_rate'] and spawner_args[spawner_keyword]['noise'] -#}

    {%- set min_distance = 0.1 -%}
    {%- set max_distance = 50.0 -%}
    {%- set image_width = 640 -%}
    {%- set image_height = 480 -%}
    {%- set horizontal_fov = 1.32 -%}

    <!-- custom monochrome camera {-->
    <link name="{{ camera_name }}_link">
      <pose>{{ x }} {{ y }} {{ z }} {{ roll }} {{ pitch }} {{ yaw }}</pose>
      {{ mrs_generic.zero_inertial_macro() }}
      <sensor name="{{ camera_name }}_sensor" type="camera">
        <update_rate>{{ spawner_args[spawner_keyword]['update_rate'] }}</update_rate>
        <camera>
          <horizontal_fov>{{ horizontal_fov }}</horizontal_fov>
          <image>
            <width>{{ image_width }}</width>
            <height>{{ image_height }}</height>
            <format>L_INT8</format> <!-- this makes the data grayscale -->
          </image>
          <clip>
            <near>{{ min_distance }}</near>
            <far>{{ max_distance }}</far>
          </clip>
          <noise>
            <type>gaussian</type>
            <mean>0</mean>
            <stddev>{{ spawner_args[spawner_keyword]['noise'] }}</stddev>
          </noise>
        </camera>
        <plugin name="{{ camera_name }}_plugin" filename="libgazebo_ros_camera.so">
          <alwaysOn>true</alwaysOn>
          <updateRate>{{ spawner_args[spawner_keyword]['update_rate'] }}</updateRate>
          <cameraName>/{{ spawner_args['name'] }}/{{ camera_name }}</cameraName>
          <imageTopicName>/{{ spawner_args['name'] }}/{{ camera_name }}/image_raw</imageTopicName>
          <cameraInfoTopicName>/{{ spawner_args['name'] }}/{{ camera_name }}/camera_info</cameraInfoTopicName>
          <frameName>/{{ spawner_args['name'] }}/{{ camera_name }}_link</frameName>
          <hackBaseline>0.07</hackBaseline>
          <distortionK1>0.0</distortionK1>
          <distortionK2>0.0</distortionK2>
          <distortionK3>0.0</distortionK3>
          <distortionT1>0.0</distortionT1>
          <distortionT2>0.0</distortionT2>
        </plugin>
      </sensor>
      <visual name="{{ camera_name }}_visual">
        <pose>0 0 0 0 0 0</pose>
        <geometry>
          <box>
            <size>0.03 0.022 0.015</size>
          </box>
        </geometry>
        <material>
          <script>
            <name>Gazebo/Yellow</name>
            <uri>file://media/materials/scripts/gazebo.material</uri>
          </script>
        </material>
      </visual>
      <visual name="{{ camera_name }}_lens_visual">
        <pose>0.014 0 0 0 0 0</pose>
        <geometry>
          <sphere>
            <radius>0.006</radius>
          </sphere>
        </geometry>
        <material>
          <script>
            <name>Gazebo/DarkGrey</name>
            <uri>file://media/materials/scripts/gazebo.material</uri>
          </script>
        </material>
      </visual>
    </link>
    <joint name="{{ camera_name }}_joint" type="fixed">
      <parent>{{ parent_link }}</parent>
      <child>{{ camera_name }}_link</child>
    </joint>
    <!--}-->

  {%- endif -%}
{%- endmacro -%}
{# <!--}--> #}

</sdf>
